#include "path_planning/dijkstra.h"

#include <climits>
#include <iostream>
#include <queue>

#include "graph/graph.h"

bool dijkstra(const std::vector<std::vector<std::pair<int, int>>>& neighbourTables,
              const int target, int& distance)
{
    std::vector<int> distances(neighbourTables.size(), INT_MAX);
    std::priority_queue<GraphNode, std::vector<GraphNode>, GraphNodeCompare> pq;

    distances[0] = 0;
    pq.push(GraphNode(0, 0));

    while (!pq.empty())
    {
        GraphNode node = pq.top();
        pq.pop();

        const int id = node.id;
        if (node.distance != distances[id])
        {
            std::cout << "节点" << char('A' + id) << "已过期或者已经访问过" << std::endl;
            continue;
        }

        if (target == id)
        {
            std::cout << "找到目标节点" << std::endl;
            distance = node.distance;
            return true;
        }

        // 对计算好的最短路径的节点放松邻居
        for (const auto& neighbour : neighbourTables[id])
        {
            const int neighbourId = neighbour.first;
            const int newWeight = node.distance + neighbour.second;

            if (newWeight < distances[neighbourId])
            {
                std::cout << "节点" << char('A' + neighbourId)
                          << "被更新，更新前的距离: " << distances[neighbourId] << ", 更新后： "
                          << newWeight << std::endl;
                distances[neighbourId] = newWeight;

                pq.push(GraphNode(neighbourId, newWeight));
            }
        }
    }

    // for (int i = 0; i < distances.size(); i++)
    // {
    //     std::cout << "节点" << char('A' + i) << "的距离: " << distances[i] << std::endl;
    // }

    return false;
}